﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_MergeDynObjectList.cs
 * \brief	This file contains the ObjectIdentification function that 
 *  
 * This file contains the ObjectIdentification function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.03
 * \date      2010-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ                          2010-02-16 \n
 *      First implementation of the M44_MergeDynObjectList class.
 *
 * - Version 0.02:   Bjoern STEURER                             2010-04-22 \n
 *      Moved to M44_SharedClasses, namespace changed accordingly
 *  
 *  -Version 0.03: Gonzalo RODRIGUEZ                          2010-06-23 \n
 *      Add the constants that control the whole dynamic object detection algorithm
 *      
 *      
/* ######################################################################### */
using System;
/*This is for using lists*/
using System.Collections;
using System.Collections.Generic;
using System.Text;
/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Basis;
using Interfaces.Complex;
/*Using the C Sharp Matrix Library*/
using NMRS.CSML;
/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;

/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SharedClasses
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SharedClasses
{

    public struct DynObj_Const
    {
        //Smallest difference between two laser measurements in the same point of the matrix 
        //from which you mark the point as changed (in meters)
        public const float f_laser_laser_dif_tol = 0.0125f;
        //Percentage of the scan from which you decide that too much has changed and the object recognition is not valid
        //eg. If more thatn this percentage of pixels has changed something is wrong and you don't perform the algorithm
        public const float f_laser_scan_percentage_tol = 0.3f;
        //Minimum number of pixels that an object needs to be consider a dynamic object
        public const int i_min_number_pixel = 5;
        //Difference between the laser measurement and the reference from which you consider that the pixel 
        //is not part of the object but of the background (in meters)
        public const float f_laser_reference_dif_tol = 0.9f;
        //Tolerance in object sizes (in meters)
        public const float f_x_size_tol = 1;
        public const float f_y_size_tol = 1;
        public const float f_z_size_tol = 1;
        //Distance tolerance between two objects in order to consider that the selected pair is viable and is the 
        //same object (in meters)
        public const float f_object_distance_tol = 100;
        //Maximum time that an object remains in the dynamic object list without being detected (in seconds)
        public const float t_min_detect = 10;
    }
    /* ------------------------------------------------------------------------- */
    /**	\class M44_ObjectDetection
     * 
     *  \brief  This is the class of the M44_ObjectIdentification function.
     *
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_MergeDynObjectList
    {
        

        /* ------------------------------------------------------------------------- */
        /**	\fn public static RobotObjectList MergeDynObjectLists(RobotObjectList str_RobotObjectList,
                                                           cl_DynamicObjects str_DetectedDynObjects)
         * 
         *  \brief This function finds pairs the objects find in the current iteration with the global list 
         *          of object from this robot
         *     
         *  \param[in] RobotObjectList str_RobotObjectList  current list of object and properties
         *  \param[in] cl_DynamicObjects str_DetectedDynObjects      global list of objects and properties
         *  
         *  \param[out] RobotObjectList str_RobotObjectList
         *
         */
        /* ------------------------------------------------------------------------- */
        public static cl_DynamicObjects MergeDynObjectLists(cl_DynamicObjects str_RobotObjectList,
                                                            cl_DynamicObjects str_DetectedDynObjects)

        {
            
            int n_det;
            int n_list;
            double[,] det_pos, list_pos, posible_pairs, selected_pairs;
            bool first = true;
            double distance = 0;
            int i_aux;
            double dist_min;
            double distance_tol = DynObj_Const.f_object_distance_tol;
            bool[] obj_det_paired;
            bool[] obj_list_paired;
            float DetTime, ListTime;
            double[] pos1 = new double[2];
            int max_id = 0;
            cl_DynamicObjects DynObjAux = new cl_DynamicObjects();
            int n_pairs_ok = 0;
            n_det = str_DetectedDynObjects.List.Length;
            obj_det_paired = new bool[n_det];
            n_list = str_RobotObjectList.List.Length;
            obj_list_paired = new bool[n_list];

            //Pair the objects in both lists
            if (n_det == 0 && n_list > 0)
            {
                DynObjAux = str_RobotObjectList;
            }
            else if (n_list > 0 && n_det == 0)
            {
                DynObjAux = str_DetectedDynObjects;
            }
            else if (n_det > 0 && n_list > 0)
            {
                det_pos = new double[n_det, 2];
                list_pos = new double[n_list, 2];
                posible_pairs = new double[n_list * n_det, 3];
                selected_pairs = new double[Math.Min(n_det, n_list), 3];

                //Calculate the distance between the points
                for (int i = 0; i < n_det; i++)
                {
                    det_pos[i, 0] = str_DetectedDynObjects.List[i].Position.d_x;
                    det_pos[i, 1] = str_DetectedDynObjects.List[i].Position.d_y;
                    for (int j = 0; j < n_list; j++)
                    {
                        if (first)
                        {
                            list_pos[j, 0] = str_RobotObjectList.List[j].Position.d_x;
                            list_pos[j, 1] = str_RobotObjectList.List[j].Position.d_y;
                        }
                        //Calculate the distance
                        distance = Math.Sqrt((Math.Pow((list_pos[j, 0] - det_pos[i, 0]), 2) +
                                              Math.Pow((list_pos[j, 1] - det_pos[i, 1]), 2)));
                        //Save the pair
                        posible_pairs[j + i * n_det, 0] = i;
                        posible_pairs[j + i * n_det, 1] = j;
                        posible_pairs[j + i * n_det, 2] = distance;
                    }
                    first = false;
                }
                //Order the pairs
                dist_min = 1000000;
                i_aux = 0;
                while (i_aux < Math.Min(n_det, n_list))
                {
                    for (int i = 0; i < (n_det * n_list); i++)
                    {
                        if (posible_pairs[i, 2] < dist_min && posible_pairs[i, 2] > 0)
                        {
                            selected_pairs[i_aux, 0] = posible_pairs[i, 0];
                            selected_pairs[i_aux, 1] = posible_pairs[i, 1];
                            selected_pairs[i_aux, 2] = posible_pairs[i, 2];
                            dist_min = posible_pairs[i, 2];
                        }
                    }
                    for (int i = 0; i < (n_det * n_list); i++)
                    {
                        if (posible_pairs[i, 0] == selected_pairs[i_aux, 0])
                            posible_pairs[i, 2] = -1;
                        if (posible_pairs[i, 1] == selected_pairs[i_aux, 1])
                            posible_pairs[i, 2] = -1;
                    }
                    dist_min = 1000000;
                    i_aux++;

                }
                //Check each pair to see if distance its acceptable and to merge the data
                for (int i = 0; i < Math.Min(n_det, n_list); i++)
                {
                    if (selected_pairs[i, 2] < distance_tol)
                    {
                        n_pairs_ok++;
                    }
                }
                DynObjAux = new cl_DynamicObjects((ushort)(n_list + n_det - n_pairs_ok));
                i_aux = 0;
                while (i_aux < n_list + n_det - n_pairs_ok)
                {
                    for (int i = 0; i < Math.Min(n_det, n_list); i++)
                    {
                        if (selected_pairs[i, 2] < distance_tol)
                        {
                            ListTime = str_RobotObjectList.List[(int)selected_pairs[i, 1]].f_DetectionTime;
                            DetTime = str_DetectedDynObjects.List[(int)selected_pairs[i, 0]].f_DetectionTime;
                            obj_det_paired[(int)selected_pairs[i, 0]] = true;
                            obj_list_paired[(int)selected_pairs[i, 1]] = true;
                            if (DetTime > ListTime)
                            {
                                DynObjAux.List[i_aux] = str_DetectedDynObjects.List[(int)selected_pairs[i, 0]];
                                DynObjAux.List[i_aux].us_Identification = str_RobotObjectList.List[(int)selected_pairs[i, 1]].us_Identification;
                            }
                            else
                                DynObjAux.List[i_aux] = str_RobotObjectList.List[(int)selected_pairs[i, 1]];

                            max_id = Math.Max(max_id, (int)DynObjAux.List[i_aux].us_Identification);
                            i_aux++;
                        }
                    }
                    for (int i = 0; i < n_list; i++)
                    {
                        if (!obj_list_paired[i])
                        {
                            DynObjAux.List[i_aux] = str_RobotObjectList.List[i];
                            max_id = Math.Max(max_id, (int)DynObjAux.List[i_aux].us_Identification);
                            i_aux++;
                        }
                    }
                    for (int i = 0; i < n_det; i++)
                    {
                        if (!obj_det_paired[i])
                        {
                            DynObjAux.List[i_aux] = str_DetectedDynObjects.List[i];
                            DynObjAux.List[i_aux].us_Identification = (ushort)(max_id + 1);
                            max_id++;
                            i_aux++;
                        }
                    }
                }
            }
            else
            {
                DynObjAux = str_RobotObjectList;
            }
              return DynObjAux;
        }
    }
}
